#include <assert.h>
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
#include <iomanip>
#include <fstream>
#include <iostream>
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
#include <BasicDagLibrary.h>
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
using namespace std;
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
U64 XtoTheN( const U64 x, U64 n ) {
   U64 r = 1ULL;
   while( n ) {
      r *= x;
      n--;
   }
   return r;
}
U64 LogXCeil( U64 v, U64 base ) {
   if( base == 2ULL ) {
      return Log2Ceil( v );
   }
   else if( base == 4 ) {
      U64 trial = Log2Ceil( v ) >> 1;
      U64 count = trial;
      U64 test  = 1ULL;
      while( count ) {
         test *= 4;
         count--;
      }
      if( test < v )
         return trial + 1;
      return trial;
   }
   else if( base == 8 ) {
      U64 trial = Log2Ceil( v ) / 3;
      if( XtoTheN( 8, trial ) < v ) {
         trial++;
         assert( !(XtoTheN( 8, trial ) < v ) );
      }
      return trial;
   }
   else {
      assert( false );
   }
}
U64 Log2Ceil( U64 v ) {
   
   U64 sa = 0ULL;
   bool add = false;
   
   while( v > 1 ) {
      add |= v & 1ULL;
      v >>= 1ULL;
      sa++;
   }
   if( add ) { sa++; }
   return sa;
   
}
U64 RoundUpToPowX( U64 v, U64 base ) {
   U64 sa = LogXCeil( v, base );
   v = 1ULL;
   while( sa ) {
      v *= base;
      sa--;
   }
   return v;
}
U64 RoundUpToPow2( U64 v ) {
   U64 sa = Log2Ceil( v );
   return (1ULL << sa);
}
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void Pos2RowColQTSbd( U32 & r, U32 & c, U32 m, U32 n, U64 pos ) {
   if( m==1 and n==1 ) { return; }
   m >>= 1;
   n >>= 1;
   U64 nel = (U64)m * (U64)n;
   if( nel == 0 ) { ASSERT( false ); }
   U32 quad = 0;
   while( pos >= nel ) {
      quad += 1;
      pos  -= nel;
   }
   ASSERT( pos >= 0 );
   if     ( quad == 0 ) { c += n;         Pos2RowColQTSbd( r, c, m, n, pos ); }
   else if( quad == 1 ) {                 Pos2RowColQTSbd( r, c, m, n, pos ); }
   else if( quad == 2 ) { r += m; c += n; Pos2RowColQTSbd( r, c, m, n, pos ); }
   else if( quad == 3 ) { r += m;         Pos2RowColQTSbd( r, c, m, n, pos ); }
   else                 { ASSERT( false );                                    }
}
void Pos2RowColQTSad( U32 & r, U32 & c, U32 m, U32 n, U64 pos ) {
   if( m==1 and n==1 ) { return; }
   m >>= 1;
   n >>= 1;
   U64 nel = (U64)m * (U64)n;
   if( nel == 0 ) { ASSERT( false ); }
   U32 quad = 0;
   while( pos >= nel ) {
      quad += 1;
      pos  -= nel;
   }
   ASSERT( pos >= 0 );
   if     ( quad == 0 ) { r += m;         Pos2RowColQTSad( r, c, m, n, pos ); }
   else if( quad == 1 ) {                 Pos2RowColQTSad( r, c, m, n, pos ); }
   else if( quad == 2 ) { r += m; c += n; Pos2RowColQTSad( r, c, m, n, pos ); }
   else if( quad == 3 ) { c += n;         Pos2RowColQTSad( r, c, m, n, pos ); }
   else                 { ASSERT( false );                                    }
}
void Pos2RowColQTSod( U32 & r, U32 & c, U32 m, U32 n, U64 pos ) {
   if( m==1 and n==1 ) { return; }
   ASSERT( m == n );
   ASSERT( m > 1 );
   ASSERT( n > 1 );
   ASSERT( m % 2 == 0 );
   m >>= 1;
   n >>= 1;
   U64 nel = (U64)m * (U64)n;
   U32 quad = 0;
   while( pos >= nel ) {
      quad += 1;
      pos  -= nel;
   }
   ASSERT( pos >= 0 );
   if     ( quad == 0 ) {                 Pos2RowColQTSod( r, c, m, n, pos ); }
   else if( quad == 1 ) { r += m; c += n; Pos2RowColQTSod( r, c, m, n, pos ); }
   else if( quad == 2 ) { r += m;         Pos2RowColQTSbd( r, c, m, n, pos ); }
   else if( quad == 3 ) { c += n;         Pos2RowColQTSad( r, c, m, n, pos ); }
   else                 { ASSERT( false );                                    }
}
void Pos2RowColQTS( U32 & r, U32 & c, U32 m, U32 n, U64 pos ) {
   if( m==1 and n==1 ) { return; }
   if( m > n ) {
      m >>= 1;
      U64 nel = (U64)m * (U64)n;
      U32 half = 0;
      while( pos >= nel ) {
         half += 1;
         pos  -= nel;
      }
      ASSERT( pos >= 0 );
      if     ( half == 0 ) {         Pos2RowColQTS( r, c, m, n, pos ); }
      else if( half == 1 ) { r += m; Pos2RowColQTS( r, c, m, n, pos ); }
      else                 { ASSERT( false );                          }
   }
   else if( n > m ) {
      n >>= 1;
      U64 nel = (U64)m * (U64)n;
      U32 half = 0;
      while( pos >= nel ) {
         half += 1;
         pos  -= nel;
      }
      ASSERT( pos >= 0 );
      if     ( half == 0 ) {         Pos2RowColQTS( r, c, m, n, pos ); }
      else if( half == 1 ) { c += n; Pos2RowColQTS( r, c, m, n, pos ); }
      else                 { ASSERT( false );                          }
   }
   else {
      ASSERT( m == n );
      m >>= 1;
      n >>= 1;
      U64 nel = (U64)m * (U64)n;
      U32 quad = 0;
      // DEBUG_PRINT( "m = " << m << ", n = " << n << ", pos = " << pos << ", nel = " << nel );
      while( pos >= nel ) {
         quad += 1;
         pos  -= nel;
      }
      // DEBUG_PRINT( "quad = " << quad );
      ASSERT( pos >= 0 );
      if     ( quad == 0 ) {                  Pos2RowColQTSod( r, c, m, n, pos ); }
      else if( quad == 1 ) { r += m; c += n;  Pos2RowColQTSod( r, c, m, n, pos ); }
      else if( quad == 2 ) { r += m;          Pos2RowColQTSbd( r, c, m, n, pos ); }
      else if( quad == 3 ) { c += n;          Pos2RowColQTSad( r, c, m, n, pos ); }
      else                 { ASSERT( false );                                     }
   }
}
U32 Pos2RowQTS( const U64 _pos, const U32 _m, const U32 _n ) {
   U32 r = 0;
   U32 c = 0;
   Pos2RowColQTS( r, c, _m, _n, _pos );
   return 1 + r;
}
U32 Pos2ColQTS( const U64 _pos, const U32 _m, const U32 _n ) {
   U32 r = 0;
   U32 c = 0;
   Pos2RowColQTS( r, c, _m, _n, _pos );
   return 1 + c;
}
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
U64 Pos2RowRMA( U64 pos, U64 m, U64 n ) {
   return 1 + pos / n;
}
U64 Pos2ColRMA( U64 pos, U64 m, U64 n ) {
   return 1 + pos % n;
}
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
U64 RC2PosQTSInner( U64 r, U64 c, U64 m, U64 p, bool bd, bool ad ) {
   // r: current row
   // c: current col
   // m: current mtx dimension
   // p: current offset (eventually the return value)
   // bd: if true, then in below-diagonal branch
   // ad: if true, then in above-diagonal branch
   if( m > 1 ) {
      m >>= 1;
      if( r < m ) {
         if( c < m ) {
            // A11
            if( bd || ad ) { p += m*m; }
         }
         else {
            // A12
            c -= m;
            if( ad ) { p += 3*m*m; }
            else {
               if( !bd ) {
                  ad = true;
                  p += 3*m*m;
               }
            }
         }
      }
      else {
         // bottom half of matrix:  A21 or A22
         r -= m;
         if( c < m ) {
            // A21
            if( bd ) { p += 3*m*m; }
            else {
               if( !ad ) {
                  bd = true;
                  p += 2*m*m;
               }
            }
         }
         else {
            // A22
            c -= m;
            if( ad || bd) { p += 2*m*m; }
            else          { p += 1*m*m; }
         }
      }
      p = RC2PosQTSInner( r, c, m, p, bd, ad );
   }
   return p;
}
U64 RC2PosQTS( U64 r, U64 c, U64 m, U64 n ) {
   U64 p = 0ULL;
   while( n > m ) {
      // matrix is fat
      n >>= 1ULL;
      if( c < n ) continue;
      else {
         c -= n;
         p += m*n;
      }
   }
   while( m > n ) {
      // matrix is skinny
      m >>= 1ULL;
      if( r < m ) continue;
      else {
         r -= m;
         p += m*n;
      }
   }
   return RC2PosQTSInner( r, c, m, p, false, false );
}
U64 RC2PosPSA( U64 r, U64 c, U64 m, U64 n ) {
   return c + n*r;
}
U64 RC2Pos( U64 r, U64 c, U64 m, U64 n, const std::string & fmt ) {
   if      ( fmt == "QTS" ) return RC2PosQTS( r, c, m, n );
   else if ( fmt == "RMA" ) return RC2PosPSA( r, c, m, n );
   else if ( fmt == "CSR" ) return RC2PosPSA( r, c, m, n );
   else if ( fmt == "COO" ) return RC2PosPSA( r, c, m, n );
   else assert( false );
}
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void PrintMappedMatrixToFile( const map< Coordinate, double > & mtx, const MatrixProperties & mtxMetaData, const string & filename ) {

   map< Coordinate, double >::const_iterator it;
   
   U64 r;
   U64 c;   

   ofstream file( filename.c_str() );
   
   file << "%%MatrixMarket matrix coordinate real general" << endl;
   file << mtxMetaData.m << " " << mtxMetaData.n << " " << mtxMetaData.nnz << endl;
   
   //for( U32 idx = 0; idx < nnz; idx++ ) {
   for( it = mtx.begin(); it != mtx.end(); it++ ) {

      r = it->first.row;
      c = it->first.col;
      file << r << " " << c << " " << setprecision( 20 ) << it->second << endl;
   }
   file.close();
}
